#include "RigidBodyPayload.h"
#include <btBulletDynamicsCommon.h>
#include <glm/gtc/type_ptr.hpp>
#include <SEPhysics/Util/Util.h>
#include <SEPhysics/Entity/BoxShape.h>
#include <SEMath/Util/Util.h>
namespace SEngine
{
    RigidBodyPayload::RigidBodyPayload()
    {
    }
    
    RigidBodyPayload::~RigidBodyPayload()
    {
    }
    
    glm::mat4 RigidBodyPayload::Transform()
    {
        btTransform trans = m_body->getWorldTransform();
        return glm::translate(glm::mat4{1.0f}, VecCast<glm::vec3>(trans.getOrigin())) * glm::mat4_cast(VecCast<glm::quat>(trans.getRotation()));
    }
    
    void RigidBodyPayload::Transform(glm::mat4 &matrix)
    {
        btTransform trans = btTransform::getIdentity();
        Decomposer m(matrix);
        trans.setOrigin(VecCast<btVector3>(m.translate));
        trans.setRotation(VecCast<btQuaternion>(m.quat));
        m_body->setWorldTransform(trans);
    }

    void RigidBodyPayload::ApplyImpulse(const glm::vec3 &impulse, const glm::vec3 &rel_pos)
    {
        m_body->applyImpulse(VecCast<btVector3>(impulse), VecCast<btVector3>(rel_pos));
    }

    void RigidBodyPayload::ApplyCentralImpulse(const glm::vec3 &impulse)
    {
        m_body->applyCentralImpulse(VecCast<btVector3>(impulse));
    }
    
    void RigidBodyPayload::SetLinearVelocity(const glm::vec3 &v)
    {
        m_body->setLinearVelocity(VecCast<btVector3>(v));
    }

    glm::vec3 RigidBodyPayload::GetLinearVelocity()
    {
        return VecCast<glm::vec3>(m_body->getLinearVelocity());
    }

    void RigidBodyPayload::SetLinearFactor(const glm::vec3 &v)
    {
        m_body->setLinearFactor(VecCast<btVector3>(v));
    }

    void RigidBodyPayload::SetAngularVelocity(const glm::vec3 &v)
    {
        m_body->setAngularVelocity(VecCast<btVector3>(v));
    }

    glm::vec3 RigidBodyPayload::GetAngularVelocity()
    {
        return VecCast<glm::vec3>(m_body->getAngularVelocity());
    }

    void RigidBodyPayload::SetAngularFactor(const glm::vec3 &v)
    {
        m_body->setAngularFactor(VecCast<btVector3>(v));
    }

    void RigidBodyPayload::SetFriction(float friction)
    {
        m_body->setFriction(friction);
    }
    
    void RigidBodyPayload::SetRestitution(float rest)
    {
        m_body->setRestitution(rest);
    }

    void RigidBodyPayload::SetCollisionFlags(int flags)
    {
        m_body->setCollisionFlags(flags);
    }

    int RigidBodyPayload::GetCollisionFlags()
    {
        return m_body->getCollisionFlags();
    }

    void RigidBodyPayload::AddCollisionFlag(int flag)
    {
        m_body->setCollisionFlags(m_body->getCollisionFlags() | flag);
    }

    void RigidBodyPayload::RemoveCollisionFlag(int flag)
    {
        m_body->setCollisionFlags(m_body->getCollisionFlags() & ~flag);
    }

    void RigidBodyPayload::SetMass(float mass, glm::vec3 inertia)
    {
        m_body->setMassProps(mass, VecCast<btVector3>(inertia));
    }

    void RigidBodyPayload::SetMass(float mass)
    {
        btVector3 inertia;
        if (mass == 0.0f) {
            inertia = btVector3{0.0f, 0.0f, 0.0f};
        } else {
            m_shape->Shape()->calculateLocalInertia(mass, inertia);
        }
        m_body->setMassProps(mass, inertia);
    }

    void RigidBodyPayload::SetKinematic(bool value)
    {
        // SetMass()
    }

    void RigidBodyPayload::SetTrigger(bool value)
    {
        if (value)
            AddCollisionFlag(btCollisionObject::CF_NO_CONTACT_RESPONSE);
        else
            RemoveCollisionFlag(btCollisionObject::CF_NO_CONTACT_RESPONSE);
    }

    void RigidBodyPayload::SetShape(std::unique_ptr<BaseShape> && shape)
    {
        m_shape = std::move(shape);
        m_body->setCollisionShape(m_shape->Shape());
    }

    void RigidBodyPayload::
    CreateBody(BodySetting & setting)
    {
        m_shape = std::move(setting.shape);
        Decomposer startTransform(setting.startTransform);
        m_state = std::make_unique<btDefaultMotionState>(btTransform{VecCast<btQuaternion>(startTransform.quat), VecCast<btVector3>(startTransform.translate)});
        btVector3 inertia{0.0f, 0.0f, 0.0f};
        if (setting.mass != 0.0f) {
            m_shape->Shape()->calculateLocalInertia(setting.mass, inertia);
        } 
        m_shape->Shape()->setLocalScaling(VecCast<btVector3>(startTransform.scale));
        btRigidBody::btRigidBodyConstructionInfo info{setting.mass, m_state.get(), m_shape->Shape(), inertia};
        m_body = std::make_unique<btRigidBody>(info);
        SetLinearVelocity(setting.linearVelocity);
        SetAngularVelocity(setting.angularVelocity);
        SetLinearFactor(setting.linearFactor);
        SetAngularFactor(setting.angularFactor);
        SetFriction(setting.friction);
        SetRestitution(setting.restitution);
        AddCollisionFlag(btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
        m_body->setActivationState(DISABLE_DEACTIVATION);

        SignalAddBody.Invoke(m_body.get());
    }

    void RigidBodyPayload::DestroyBody()
    {
        SignalRemoveBody.Invoke(m_body.get());
        m_state.reset();
        m_shape.reset();
        m_body.reset();
    }

} // namespace SEngine
